 package bcit.nxt.mission;

// PathFinder.java

import gin.and.tonic.*;
import lejos.nxt.Button;

public class PathFinder
{
  private final boolean debug = false;

  public PathFinder()
  {
    NxtRobot robot = new NxtRobot(true);

    LightSensor rightEye = new LightSensor(SensorPort.S4);
    LightSensor leftEye = new LightSensor(SensorPort.S3);
    robot.addPart(rightEye);
    robot.addPart(leftEye);
    rightEye.activate(true);
    leftEye.activate(true);

    Gear gear = new Gear(MotorPort.B,MotorPort.C);
    robot.addPart(gear);
    gear.setSpeed(20);
    gear.forward();

    while (!Button.ENTER.isPressed())
    {
      int leftValue  = leftEye.getValue();
      int rightValue = rightEye.getValue();
      int difference = leftValue - rightValue;
      if (debug)
      {
        System.out.println(leftValue + " | " + rightValue + " | " + difference);
        Tools.delay(300);
      }

      if (difference <- 100)                        // right darker (off path)
         gear.leftArc(0.1);                        // turn left

      if (difference >100)                       // left darker (off path)
        gear.rightArc(0.1);                        // turn right

      if (difference > -100 && difference < 100)   // both similar
      {
        if (leftValue < 300)                       // both bright (on path)
          gear.forward();                          // go straight forward
        else                                       // both dark (off path)
          gear.backward();                         // go straight backward
      }
    }
    robot.exit();
  }

  public static void main(String[] args)
  {
    new PathFinder();
  }
}